#include "image_proc.h"
#include <QDebug>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#define PI  3.141592654

/* 比较函数，供qsort升序使用 */
static int comp_up(const void *a, const void *b)
{
    return *(unsigned char*)a - *(unsigned char*)b;
}

/* 中值滤波，3*3 */
int median_filter(unsigned char *src, unsigned char *dest, int width, int height)
{
    int ret;
    unsigned char wind[3 * 3];
    int i, j, row, col;
    unsigned char *buf;

    buf = (unsigned char *)malloc(width * height);
    if (buf == NULL) {
        ret = OUT_OF_MEMORY;
	goto err_1;
    }

    for (i = 1; i < height - 1; i++) {
        for (j = 1; j < width - 1; j++) {
            for (row = 0; row < 3; row++) {
                for (col = 0; col < 3; col++) {
                    wind[row * 3 + col] = src[(i + row - 1) * width + (j + col - 1)];
                }
            }
            qsort((void *)wind, 9, sizeof(unsigned char), comp_up);
            buf[i * width + j] = wind[4];
        }
    }

    memcpy(dest, buf, width * height);
    free(buf);

err_1:
    return ret;
}

/* 均值滤波，3*3 */
void average_filter(unsigned char *src, unsigned char *dest, int width, int height)
{
    int i, j, row, col;
    int sum;
    unsigned char *buf;

    buf = (unsigned char *)malloc(width * height);
    /* if error */

    for (i = 1; i < height - 1; i++) {
        for (j = 1; j < width - 1; j++) {
            sum = 0;
            for (row = 0; row < 3; row++) {
                for (col = 0; col < 3; col++) {
                    sum += src[(i + row - 1) * width + (j + col - 1)];
                }
            }
            buf[i * width + j] = sum / 9;
        }
    }

    memcpy(dest, buf, width * height);
    free(buf);
}

/* 高斯滤波，5*5 */
void gaussian_filter(unsigned char *src, unsigned char *dest, int width, int height, double sigma)
{
    double _template[5][5];
    long long int gaussian_template[5][5];
    int i, j, row, col;
    double x, y, k;
    long long int sum, gaussian_sum;
    unsigned char *buf;

    sigma = sigma < 0.5 ? 0.5 : sigma;
    sigma = sigma > 3.2 ? 3.2 : sigma;

    /* Start：生成高斯滤波模板 */
    for (i = 0; i < 5; i++) {
        x = pow(i - 2, 2);
        for (j = 0; j < 5; j++) {
            y = pow(j - 2, 2);
            _template[i][j] = exp(-(x + y) / (2 * sigma * sigma)) / (2 * PI * sigma);
        }
    }

    /* 模板归一化处理，左上角变为1 */
    k = 1.0 / _template[0][0];
    gaussian_sum = 0;
    for (i = 0; i < 5; i++) {
        for (j = 0; j < 5; j++) {
            gaussian_template[i][j] = (int)(k * _template[i][j] + 0.5);
            gaussian_sum += gaussian_template[i][j];
//            printf("%2lld ", gaussian_template[i][j]);
        }
//        printf("\n");
    }
//    printf("sigma = %lf, k = %lf, sum = %lld\n", sigma, k, gaussian_sum);
//    fflush(NULL);
    /* End：生成高斯滤波模板 */

    buf = (unsigned char *)malloc(width * height);
    /* if error */

    /* 开始滤波 */
    for (i = 2; i < height - 2; i++) {
        for (j = 2; j < width - 2; j++) {
            sum = 0;
            for (row = 0; row < 5; row++) {
                for (col = 0; col < 5; col++) {
                    sum += gaussian_template[row][col] * src[(i + row - 2) * width + (j + col - 2)];
                }
            }
            buf[i * width + j] = sum / gaussian_sum;
        }
    }

    memcpy(dest, buf, width * height);
    free(buf);

}

/* 二值化 */
void binaryzation(unsigned char *src, unsigned char *dest, int width, int height, int bin_threshold)
{
    int i, j;
    unsigned char *buf;

    buf = (unsigned char *)malloc(width * height);
    memset(buf, 0, width * height);

    for (i = 0; i < height; i++) {
        for (j = 0; j < width; j++) {
            buf[i * width + j] = src[i * width + j] > bin_threshold ? 255 : 0;
        }
    }

    memcpy(dest, buf, width * height);
    free(buf);
}

/* 二值图像反色处理 */
void bin_img_reverse(unsigned char *src, unsigned char *dest, int width, int height)
{
    int i, j;
    unsigned char *buf;

    buf = (unsigned char *)malloc(width * height);
    memset(buf, 0, width * height);

    for (i = 0; i < height; i++) {
        for (j = 0; j < width; j++) {
            buf[i*width + j] = (src[i*width + j] == 0) ? 255 : 0;
        }
    }

    memcpy(dest, buf, width * height);
    free(buf);
}

/* 方形卷积核腐蚀，黑点为0白点为255 */
void erosion(unsigned char *src, unsigned char *dest, int width, int height)
{
    int i, j;
    int x, y, val = 0;
    unsigned char *buf;

    buf = (unsigned char *)malloc(width * height);

    for (i = 1; i < height - 1; i++) {
        for (j = 1; j < width - 1; j++) {
            val = 0;
            for (x = i - 1; x <= i + 1; x++) {
                for (y = j - 1; y <= j + 1; y++) {
                    val |= src[x * width + y];
                }
            }
            buf[i * width + j] = val;
        }
    }

    memcpy(dest, buf, width * height);
    free(buf);
}

/* 方形卷积核膨胀，黑点为0白点为255 */
void dilation(unsigned char *src, unsigned char *dest, int width, int height)
{
    int i, j;
    int x, y, val = 0;
    unsigned char *buf;

    buf = (unsigned char *)malloc(width * height);

    for (i = 1; i < height - 1; i++) {
        for (j = 1; j < width - 1; j++) {
            val = 255;
            for (x = i - 1; x <= i + 1; x++) {
                for (y = j - 1; y <= j + 1; y++) {
                    val &= src[x * width + y];
                }
            }
            buf[i * width + j] = val;
        }
    }

    memcpy(dest, buf, width * height);
    free(buf);
}

/* 开运算，先腐蚀后膨胀 */
void open_operation(unsigned char *src, unsigned char *dest, int width, int height)
{
    erosion(src, dest, width, height);
    dilation(src, dest, width, height);
}

/* 闭运算，先膨胀后腐蚀 */
void close_operation(unsigned char *src, unsigned char *dest, int width, int height)
{
    dilation(src, dest, width, height);
    erosion(src, dest, width, height);
}

/* 与运算 */
void and_operation(unsigned char *src, unsigned char *mask, unsigned char *dest, int width, int height)
{
    int i, j;
    unsigned char *buf;

    buf = (unsigned char *)malloc(width * height);
    memset(buf, 255, width * height);

    for (i = 0; i < height; i++) {
        for (j = 0; j < width; j++) {
            buf[i * width + j] = src[i * width + j] & mask[i * width + j];
        }
    }

    memcpy(dest, buf, width * height);
    free(buf);
}

/* 或运算 */
void or_operation(unsigned char *src, unsigned char *mask, unsigned char *dest, int width, int height)
{
    int i, j;
    unsigned char *buf;

    buf = (unsigned char *)malloc(width * height);
    memset(buf, 255, width * height);

    for (i = 0; i < height; i++) {
        for (j = 0; j < width; j++) {
            buf[i * width + j] = src[i * width + j] | mask[i * width + j];
        }
    }

    memcpy(dest, buf, width * height);
    free(buf);
}

/* 提高对比度，直方图均衡法 */
void increasing_contrast(unsigned char *src, unsigned char *dest, int width, int height)
{
    int histogram[GRAY_LEVELS]; //灰度直方图，每级灰度包含的点数
    int sum = 0;
    int gray_level_lut[GRAY_LEVELS];
    int i, j;
    int image_size = width * height;

    /* Start:统计灰度直方图 */
    memset(histogram, 0, GRAY_LEVELS * sizeof(int));
    for (i = 0; i < height; i++) {
        for (j = 0; j < width; j++) {
            histogram[src[i * width + j]]++;
        }
    }
    /* End:统计灰度直方图 */

    /* Start:构建直方图的累计分布方程，建立灰度值变换映射关系 */
    for (i = 0; i < GRAY_LEVELS; i++) {
        sum += histogram[i];
        gray_level_lut[i] = sum * (GRAY_LEVELS - 1) / image_size;
    }
    /* End:构建直方图的累计分布方程，建立灰度值变换映射关系 */

    for (i = 0; i < height; i++) {
        for (j = 0; j < width; j++) {
            dest[i * width + j] = gray_level_lut[src[i * width + j]];
        }
    }
}

/* 边缘检测，基于sobel算子 */
void sobel_edge_detection(unsigned char *src, unsigned char *dest, int width, int height, int edge_threshold)
{
    int sobel_operator_gx[3][3] = {
        {-1, 0, 1},
        {-2, 0, 2},
        {-1, 0, 1},
    };
    int sobel_operator_gy[3][3] = {
        { 1,  2,  1},
        { 0,  0,  0},
        {-1, -2, -1},
    };
    int gx_sum, gy_sum, sum;
    int i, j, row, col, src_coordinate;
//    int max = 0;

    memset(dest, 0, height * width);
    for (i = 1; i < height - 1; i++) {
        for (j = 1; j < width - 1; j++) {
            gx_sum = 0;
            gy_sum = 0;
            for (row = 0; row < 3; row++) {
                for (col = 0; col < 3; col++) {
                    src_coordinate = (i + row - 1) * width + (j + col - 1); //计算坐标位置
                    gx_sum += sobel_operator_gx[row][col] * src[src_coordinate];
                    gy_sum += sobel_operator_gy[row][col] * src[src_coordinate];
                }
            }
            sum = abs(gx_sum) + abs(gy_sum);
            dest[i * width + j] = sum > edge_threshold ? 255 : 0;
//            if (sum > max)
//                max = sum;
        }
    }

//    qDebug() << max;
}

/* Start：边缘检测，基于canny算法 */
/* 边缘梯度方向 */
enum {GRA_DIR0, GRA_DIR45, GRA_DIR90, GRA_DIR135};

// Start：判断是否是边缘梯度方向极大值
static int is_gradient_direction0_crest(int *edge_map, int width, int height, int x, int y)
{
    int crest;
    int j;

    if (y < 1) { //防止擦边越界
        y = 1;
    }

    if (y >= height - 1) {
        y = height - 1;
    }

    if (x >= width - 1) {
        x = width - 1;
    }

    if (x <= 1) {
        x = 1;
    }

    crest = 0;
    for (j = 0; j < 3; j++) {
        if (edge_map[y * width + x + j - 1] > crest) {
            crest = edge_map[y * width + x + j - 1];
        }
    }

    return edge_map[y * width + x] >= crest;
}

static int is_gradient_direction45_crest(int *edge_map, int width, int height, int x, int y)
{
    int crest;
    int i, j;

    if (y <= 1) { //防止擦边越界
        y = 1;
    }

    if (y >= height - 1) {
        y = height - 1;
    }

    if (x >= width - 1) {
        x = width - 1;
    }

    if (x <= 1) {
        x = 1;
    }

    crest = 0;
    for (i = 0, j = 2; i < 3; i++, j--) {
        if (edge_map[(y + i - 1) * width + x + j - 1] > crest) {
            crest = edge_map[(y + i - 1) * width + x + j - 1];
        }
    }

    return edge_map[y * width + x] >= crest;
}

static int is_gradient_direction90_crest(int *edge_map, int width, int height, int x, int y)
{
    int crest;
    int i;

    if (y <= 1) { //防止擦边越界
        y = 1;
    }

    if (y >= height - 1) {
        y = height - 1;
    }

    if (x >= width - 1) {
        x = width - 1;
    }

    if (x <= 1) {
        x = 1;
    }

    crest = 0;
    for (i = 0; i < 3; i++) {
        if (edge_map[(y + i - 1) * width + x] > crest) {
            crest = edge_map[(y + i - 1) * width + x];
        }
    }

    return edge_map[y * width + x] >= crest;
}

static int is_gradient_direction135_crest(int *edge_map, int width, int height, int x, int y)
{
    int crest;
    int i, j;

    if (y <= 1) { //防止擦边越界
        y = 1;
    }

    if (y >= height - 1) {
        y = height - 1;
    }

    if (x >= width - 1) {
        x = width - 1;
    }

    if (x <= 1) {
        x = 1;
    }

    crest = 0;
    for (i = 0, j = 0; i < 3; i++, j++) {
        if (edge_map[(y + i - 1) * width + x + j - 1] > crest) {
            crest = edge_map[(y + i - 1) * width + x + j - 1];
        }
    }

    return edge_map[y * width + x] >= crest;
}

typedef int (*fun_crest)(int *edge_map, int width, int height, int x, int y);
static fun_crest is_gradient_crest[] = {
    is_gradient_direction0_crest,
    is_gradient_direction45_crest,
    is_gradient_direction90_crest,
    is_gradient_direction135_crest,
};
// End：判断是否是边缘梯度方向极大值

// Start：滞后边界跟踪，将与强边界不连接的弱边界删除
void mark_point(unsigned char *edge_map, int width, int height, int x, int y)
{
    int i, j;

    printf("x: %d, y: %d, w: %d, h: %d\n", x, y, width, height);
    fflush(NULL);

    if ((x >= width - 1) || (y >= height - 1) || (x <= 0) || (y <= 0)) {
        return;
    }

    for (i = 0; i < 3; i++) {
        for (j = 0; j < 3; j++) {
            if (edge_map[(y+i-1)*width + (x+j-1)] == 50) {
                edge_map[(y+i-1)*width + (x+j-1)] = 255;
                mark_point(&edge_map[(y+i-1)*width + (x+j-1)], width, height, x+j-1, y+i-1);
            }
        }
    }
}

void connect_edge(unsigned char *edge_map, int width, int height, int x, int y)
{
    int i, j, a, b;

    if ((x >= width - 1) || (y >= height - 1) || (x <= 0) || (y <= 0)) {
        return;
    }

    for (i = y; i < height - 1; i++) {
        for (j = x; j < width - 1; j++) {
            if (edge_map[i*width + j] == 255) {
                for (a = 0; a < 3; a++) {
                    for (b = 0; b < 3; b++) {
                        if (edge_map[(i+a-1)*width + (j+b-1)] == 50) {
                            edge_map[(i+a-1)*width + (j+b-1)] = 255;
                            mark_point(&edge_map[(i+a-1)*width + (j+b-1)], width, height, j+b-1, i+a-1);
                        }
                    }
                }
            }
        }
    }
}
// End：滞后边界跟踪，将与强边界不连接的弱边界删除

void canny_edge_detection(unsigned char *src, unsigned char *dest, int width, int height, int edge_threshold)
{
    int sobel_operator_gx[3][3] = {
        {-1, 0, 1},
        {-2, 0, 2},
        {-1, 0, 1},
    };
    int sobel_operator_gy[3][3] = {
        { 1,  2,  1},
        { 0,  0,  0},
        {-1, -2, -1},
    };
    int gx_sum, gy_sum, sum;
    int i, j, row, col, coordinate;
    int *sobel_map; //边缘强度
    unsigned char *gradient_direction; //边缘梯度方向
    double theta; //边缘梯度角，临时变量
    int min_threshold, max_threshold; //双阀值

    /* Step1:高斯滤波，去噪 */
    gaussian_filter(src, dest, width, height, 1.4);

    /* Step2:sobel边缘强度检测 + 边缘梯度方向检测 */
    sobel_map = (int *)malloc(width * height * sizeof(int));
    /* if error */
    memset(sobel_map, 0, width * height * sizeof(int));
    gradient_direction = (unsigned char *)malloc(width * height);
    /* if error */
    memset(gradient_direction, 0, width * height);
    for (i = 1; i < height - 1; i++) {
        for (j = 1; j < width - 1; j++) {
            gx_sum = 0;
            gy_sum = 0;
            for (row = 0; row < 3; row++) {
                for (col = 0; col < 3; col++) {
                    coordinate = (i + row - 1) * width + (j + col - 1); //计算坐标位置
                    gx_sum += sobel_operator_gx[row][col] * dest[coordinate];
                    gy_sum += sobel_operator_gy[row][col] * dest[coordinate];
                }
            }
            sum = abs(gx_sum) + abs(gy_sum);
            sobel_map[i * width + j] = sum; //边缘强度

            theta = atan2(gy_sum, gx_sum); //边缘梯度方向
            if (((theta > -PI/8) && (theta <=  PI/8)) || ((theta > PI*7/8) || (theta <=  -PI*7/8))) {
                gradient_direction[i * width + j] = GRA_DIR0;
            } else if (((theta > PI/8) && (theta <=  PI*3/8)) || ((theta > -PI*7/8) && (theta <=  -PI*5/8))) {
                gradient_direction[i * width + j] = GRA_DIR45;
            } else if (((theta > PI*3/8) && (theta <=  PI*5/8)) || ((theta > -PI*5/8) && (theta <=  -PI*3/8))) {
                gradient_direction[i * width + j] = GRA_DIR90;
            } else if (((theta > PI*5/8) && (theta <=  PI*7/8)) || ((theta > -PI*3/8) && (theta <=  -PI/8))) {
                gradient_direction[i * width + j] = GRA_DIR135;
            }
        }
    }

    /* Step3:非极大值抑制，去掉非边缘 */
    for (i = 1; i < height - 1; i++) {
        for (j = 1; j < width - 1; j++) {
            if (!is_gradient_crest[gradient_direction[i * width + j]](sobel_map, width, height, j, i)) {
                sobel_map[i * width + j] = 0;
            }
        }
    }

    /* Step4:双阀值限定，寻找潜在边缘 */
    min_threshold = edge_threshold - 15;
    min_threshold = (min_threshold > 0) ? min_threshold : 0;
    max_threshold = edge_threshold + 15;
    max_threshold = (max_threshold > 0) ? max_threshold : 0x7fffffff;
    for (i = 0; i < height; i++) {
        for (j = 0; j < width; j++) {
            if (sobel_map[i * width + j] > max_threshold) {
                dest[i * width + j] = 255;
            } else if (sobel_map[i * width + j] > min_threshold) {
                dest[i * width + j] = 50;
            } else {
                dest[i * width + j] = 0;
            }
        }
    }

    /* Step5:滞后边界跟踪，抑制孤立的若边缘 */
    //connect_edge(dest, width, height, 1, 1);
}
/* End：边缘检测，基于canny算法 */

/* 霍夫变换检测固定半径的圆
 * x = x0 + r * cos(a)
 * y = y0 + r * sin(a) */
static coordinates_circle hough_circle_detection_fix_radius(unsigned char *src, int width, int height, int start_x, int start_y, int radius)
{
    int *intensity; //圆心的强度
    int i, j;
    int x0, y0;
    double radian; //弧度
    int angle; //角度
    int max_intensity = 0;
    coordinates_circle circle_center = {0,0,0,0,0};

    intensity = (int *)malloc(height * width * sizeof(int));
    memset(intensity, 0, height * width * sizeof(int));

    for (i = start_y; i < start_y + 2*AVERAGE_PUPIL_RADIUS; i++) {
        for (j = start_x; j < start_x + 2*AVERAGE_PUPIL_RADIUS; j++) {
            if (src[i * width + j] == 255) {
                for (angle = 0; angle < 360; angle++) {
                    radian = (angle * PI) / 180; //角度转弧度
                    x0 = (int)(j - radius * cos(radian) + 0.5);
                    y0 = (int)(i - radius * sin(radian) + 0.5);
                    if ((x0 > 0 && x0 < width) && (y0 > 0 && y0 < height)) {
                        intensity[y0 * width + x0]++;
                    }
                }
            }
        }
    }

    for (i = 0; i < height; i++) {
        for (j = 0; j < width; j++) {
            if (intensity[i * width + j] > max_intensity) {
                max_intensity = intensity[i * width + j];
                circle_center.x = j;
                circle_center.y = i;
                circle_center.radius = radius;
                circle_center.intensity = intensity[i * width + j];
            }
        }
    }

    free(intensity);

    return circle_center;
}

/* 直线霍夫变换 */
static void hough_line(unsigned char *src, int src_width, int src_height, unsigned char *dest, int dest_width, int dest_height)
{
    int x, y;
    int alpha, rho;
    double theta;

    memset(dest, 0, dest_width*dest_height*sizeof(unsigned char));
    for (y = 0; y < src_height; y++) {
        for (x = 0; x < src_width; x++) {
            if (src[y*src_width+x] == 255) {
                for (alpha = 0; alpha < 180; alpha++) {
                    theta = alpha * PI / 180;
                    rho = (int)(x * cos(theta) + y * sin(theta) + 0.5);

                    if ((rho >= 0) && (rho < dest_height) && (alpha >= 0) && (alpha < dest_width)) {
                            dest[rho*dest_width+alpha]++;
                    }
                }
            }
        }
    }
}

/* 缩小图像，长宽均为原来的一半 */
void shrunk_half_image(unsigned char *src, unsigned char *dest, int width, int height)
{
    int i, j, h, w;
    int dest_width;

    dest_width = width / 2;

    for (i = 0, h = 0; i < height; i += 2, h++) {
        for (j = 0, w = 0; j < width; j += 2, w++) {
            dest[h * dest_width + w] = src[i * width + j];
        }
    }
}

/* 缩小图像，长宽均为原来的1/4 */
void shrunk_two_times_image(unsigned char *src, unsigned char *dest, int width, int height)
{
    int i, j, h, w;
    int dest_width;

    dest_width = width / 4;

    for (i = 0, h = 0; i < height; i += 4, h++) {
        for (j = 0, w = 0; j < width; j += 4, w++) {
            dest[h * dest_width + w] = src[i * width + j];
        }
    }
}

/* 笛卡尔坐标到极坐标变换 */
void to_polar_coordinates(unsigned char *cartesian, int width, int height, unsigned char *polar, int radius, coordinates_circle *pole)
{
    int angle; //角度
    double radian; //弧度
    int r0; // 被遍历的极径
    int x0, y0; // 极点坐标

    x0 = pole->x;
    y0 = pole->y;

    memset(polar, 0, radius*360);
    for (angle = 0; angle < 360; angle++) {
        radian = (angle * PI) / 180; //角度转弧度
        for (r0 = pole->radius; r0 < radius; r0++) {
            polar[r0 * 360 + angle] = cartesian[(y0+(int)(r0*sin(radian)))*width + (x0+(int)(r0*cos(radian)))];
        }
    }
}
